from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import DeclareLaunchArgument,ExecuteProcess
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    px4_path = LaunchConfiguration('px4_path')

    px4_path_launch_arg = DeclareLaunchArgument(
        'px4_path',
        default_value= '/home/hpdadi/px/PX4-Autopilot',
        description='Path to PX4-Autopilot'
    )

    #编写launch文件 运行  ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --qx 0 --qy 0 --qz 0 --qw 1 --frame-id world --child-frame-id mystaticturtle

    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])
    return LaunchDescription([
        px4_path_launch_arg,

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
            launch_arguments={'uav_id': 'uav1', 'uav_pose': '0,0', 'uav_num': '0', 'px4_path': px4_path, 'sim_model':'x500_under_range'}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
            launch_arguments={'uav_id': 'uav2', 'uav_pose': '10,0', 'uav_num': '1', 'px4_path': px4_path,'sim_model':'x500_under_range'}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={'world_sdf': 'default.sdf','bridge_config':'twouav_bridge_x500_under_range.yaml'}.items()
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'world', '--child-frame-id', 'x500_under_range_0/odom'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'world', '--child-frame-id', 'x500_under_range_1/odom'],
        ),
        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'x500_under_range_0/base_footprint', '--child-frame-id', 'x500_under_range_0'],
        ),

        ExecuteProcess(
            cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', '--x', '0', '--y', '0', '--z', '0', '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1', '--frame-id', 'x500_under_range_1/base_footprint', '--child-frame-id', 'x500_under_range_1'],
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'microagent.launch.py'])
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'uav.launch.py']),
            launch_arguments={'vehicleID': 'uav1', 'local_host_mode': 'True', 'multi_simulation_mode': 'True'}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'uav.launch.py']),
            launch_arguments={'vehicleID': 'uav2', 'local_host_mode': 'True', 'multi_simulation_mode': 'True'}.items()
        ),


    ])